import numpy as np

from scipy import interpolate


def f(power, gen_loss, rho, r, w_est, cp):
    return power * (1 - gen_loss)
    - 0.5 * rho * np.pi * r ** 2 * w_est ** 3 * cp


def df(rho, r, w_est, lamb, dcplamb, cp):
    return 0.5 * rho * np.pi * r ** 2 * w_est(lamb * dcplamb - 3 * cp)

w_est_old = 0


def wind_estimation(beta, rot_speed, power):
# Variables
    cp_table = [0]
    beta_vec = [0]
    lamb_vec = [0]
    # Turbine parameters
    r = 40
    gen_loss = 0.05
    rho = 4

    dflamb = lamb_vec[1] - lamb_vec[0]
    dfcpdlamb_table = 1 / dflamb * np.diff(cp_table)
    lamb_df_cp_vec = lamb_vec[:] + 0.5 * dflamb

    max_iterations = 100
    err_tol = 0.0005

    global w_est_old
    w_est = w_est_old
    for i in xrange(max_iterations):
        lamb = rot_speed * r / w_est
        fcp = interpolate.interp2d(beta_vec, lamb_vec, cp_table)
        cp = fcp(beta, lamb)
        fdcp = interpolate.interp2d(beta_vec, lamb_df_cp_vec, dfcpdlamb_table)
        dcplamb = fdcp(beta, lamb)

        fp = f(power, gen_loss, rho, r, w_est, cp)
        dfp = df(rho, r, w_est, lamb, dcplamb, cp)
        w_est_new = w_est - fp / dfp

        if (w_est_new - w_est) < err_tol:
            w_est = w_est_new
            continue
        w_est = w_est_new

    if i == max(xrange(max_iterations)):
        print("Max iteration reached")

    if w_est > w_est_old:
        grad = w_est / w_est_old
    else:
        grad = w_est_old / w_est

    if grad < 10:
        w_hat = w_est
    else:
        w_hat = w_est_old

    if np.isnan(w_hat):
        w_hat = w_est_old

    w_est_old = w_est

    return w_hat